/**
 * @file sensor.cpp
 * @brief 
 * @author haibo.yang
 * @version 1.0
 * @date 2023-07-14
 * 
 * @copyright Copyright (c) 2023  lgim
 * 
 */
#include "comm/sensor.h"
#include <jsoncpp/json/json.h>
namespace Sensor {
// serial commuicate function implementation    
SerialComm::SerialComm(std::shared_ptr<rclcpp::Node> node, 
                        const std::string port, 
                        const int baudRate, 
                        const double &rate):
                        Communication(node), 
                        loopHz_(rate),
                        bandrate_(baudRate)
{
    serial_.setPort(port);
}

SerialComm::~SerialComm() 
{
    if (serial_.isOpen())
        serial_.close();
}

int SerialComm::init()
{
    try {
        serial_.setBaudrate(/*256000*/ bandrate_);
        serial::Timeout to = serial::Timeout::simpleTimeout(1);
        serial_.setTimeout(to);
        serial_.open();
    } catch (serial::IOException &e) {
        RCLCPP_ERROR(node_->get_logger(), "Unable to open port");
        return -1;
    }
     
    if (serial_.isOpen()) {
        RCLCPP_INFO_STREAM(node_->get_logger(), "Serial Port initialized");
    } else {
        return -1;
    }

    return 0;
}

void SerialComm::update() 
{
    int rx_length_ = 0;
    rclcpp::Rate spin_rate(loopHz_);

    while (rclcpp::ok()) {
        // ROS_DEBUG_STREAM_NAMED("SerialComm", "test...");
        size_t n = 0;
        if((n = serial_.available()))
        {
            uint8_t buffer[64];
            rx_length_ = serial_.read(buffer, n);
            if (rx_length_ > 0) {
                
                if (rx_length_ >= 12)
                {
                    for (int i=0;i<rx_length_;) {
                        // if (buffer[i] == 0x5A && buffer[i+1] == 0x0C) {
                        //     encoder_ = (buffer[i+3] << 56 & 0xFF00000000000000)
                        //     | (buffer[i+4] << 48 & 0x00FF000000000000) | (buffer[i+5] << 40 & 0x0000FF0000000000)
                        //     | (buffer[i+6] << 32 & 0x000000FF00000000) | (buffer[i+7] << 24 & 0x00000000FF000000)
                        //     | (buffer[i+8] << 16 & 0x0000000000FF0000) | (buffer[i+9] << 8 & 0x000000000000FF00)
                        //     | (buffer[i+10] & 0x00000000000000FF);
                            
                        //     // RCLCPP_DEBUG(rclcpp::get_logger("SerialComm"), "rx_length_: %d, encoder: 0x%lx", rx_length_, encoder_);
                        //     i += 12;
                        //     continue;
                        // }
                        // i++;
                    }
                }
            }
        }
        // ROS_DEBUG_STREAM_NAMED("SerialComm", "test...");
        // serial_.flush();
        spin_rate.sleep();
    }
}

void SerialComm::write(const std::string msg)
{
    // to do 
}

uint64_t SerialComm::read(const uint8_t index)
{
    return encoder_;
}

// using boost::asio::ip::tcp;
// TCP commuicate function implementation 
TcpServerComm::TcpServerComm(std::shared_ptr<rclcpp::Node> node,
                 const int portNumber, 
                 boost::asio::io_service& io_service, 
                 const double &rate):
                 Communication(node),
                 io_service_(io_service), 
                 acceptor_(io_service, boost::asio::ip::tcp::endpoint(boost::asio::ip::tcp::v4(), portNumber)), 
                 loopHz_(rate), buffer_(BUFFER_MAX_LEN)
{
    // init encoder values
    memset(encoder_, 0, sizeof(encoder_));
    acceptor_.set_option(boost::asio::ip::tcp::acceptor::reuse_address(true));
    RCLCPP_INFO_STREAM(node_->get_logger(), "Server initialize");
    init();
}

TcpServerComm::~TcpServerComm() 
{
    stop();
}


void TcpServerComm::handle_accept(const boost::system::error_code &ec, 
    std::shared_ptr<boost::asio::ip::tcp::socket> socket)
{
    if(ec) {
        RCLCPP_WARN_STREAM(node_->get_logger(), "handle accecpt: " << ec); 
        return; 
    }    
    RCLCPP_INFO_STREAM(node_->get_logger(), "Remote ip: "<<socket->remote_endpoint().address()<<"  remote port: "<< socket->remote_endpoint().port()); 
    // std::shared_ptr<std::string> pstr(new std::string("hello async world!"));
    // socket->async_write_some(boost::asio::buffer(*pstr), boost::bind(&TcpServerComm::handler_write, this, pstr, _1, _2));
    parse_ = new boost::thread(boost::bind(&TcpServerComm::handler_read, this, socket)); 
  
    start();
}

int TcpServerComm::init()
{   
    return start();
}

int TcpServerComm::start()
{
    sockPtr_.reset(new boost::asio::ip::tcp::socket(io_service_));
    RCLCPP_INFO_STREAM(node_->get_logger(),"listen...");
    try
    {
        acceptor_.async_accept(*sockPtr_, boost::bind(&TcpServerComm::handle_accept, this, boost::asio::placeholders::error, sockPtr_));
    }
    catch(const std::exception& e)
    {
        RCLCPP_ERROR_STREAM(node_->get_logger(), "acceptor: " << e.what()); 
        return -1;
    }
    
    return 0;
}

unsigned char TcpServerComm::crc8(unsigned char *message, unsigned char len) 
{
    uint8_t crc;
    uint8_t i;
    crc = 0;
    while (len--) {
        crc ^= *message++;
        for (i = 0; i < 8; i++) {
            if (crc & 0x01) {
                crc = (crc >> 1) ^ 0x8c;
            } else
                crc >>= 1;
        }
    }
    return crc;
}

void TcpServerComm::check_zero_crossing(void) 
{
    for(int i=0;i<2;i++) {
        if (encoder_[i].value > encoder_[i].last_value) {
            // VALUE_MAX -> 0 Incremental overflow
            if (encoder_[i].value - encoder_[i].last_value > (0xFFFF0000 - 0xFFFF)) {
                encoder_[i].turns--;
                encoder_[i].dir == Reverse;
            }   
            encoder_[i].dir == Forward;
        } else if (encoder_[i].value < encoder_[i].last_value) {
            // 0 -> VALUE_MAX Decremented overflow
            if (encoder_[i].last_value - encoder_[i].value > (0xFFFF0000 - 0xFFFF)) {
                encoder_[i].turns++;
                encoder_[i].dir == Forward;
            }
            encoder_[i].dir == Reverse;
        } 
        encoder_[i].last_value = encoder_[i].value;
    }
}

void TcpServerComm::ParsePacket()
{   
    Packet packet;
    uint8_t header[2] = {0xAA, 0xAA};
    uint8_t tail[2] = {0x55, 0x55};
    size_t size = sizeof(header);

    while (buffer_.size() > PACKHEAD_LENGTH) {
        auto it = std::search(buffer_.begin(), buffer_.end(), std::begin(header), std::end(header));
        if (it == buffer_.end()) {
            // Header not found
            return;
        }

        // memset(packet, 0, sizeof(packet));
        auto p = it;
        // Read the packet head
        std::memcpy(&packet.header, &(*p), size);
        
        p += size; // point to seq
        // Read the device seq
        std::memcpy(&packet.seq, &(*p), size);
        
        p += size; // point to size
        // Read the packet size
        std::memcpy(&packet.size, &(*p), size);
        
        p += size; // point to payload
        // Check if there's enough data in the buffer to read the entire packet
        if (std::distance(p, buffer_.end()) < packet.size + size * 2) {
            // Not enough data
            return;
        }
        // Read the payload
        packet.payload.clear();
        packet.payload.resize(packet.size);
        std::memcpy(packet.payload.data(), &(*p), packet.size);
        
        p += packet.size; // point to crc
        // Read the crc
        std::memcpy(&packet.crc, &(*p), size);
        
        p += size;       // point to tail
        // Read the tail
        std::memcpy(&packet.tail, &(*p), sizeof(packet.tail));
        // Verify the header, tail, and crc
        if (packet.tail == 0x5555) {
            // Invalid header or tail
            uint16_t crc = crc8(reinterpret_cast<uint8_t *>(&packet.payload[0]), packet.size);
            if (crc == packet.crc) {
                // RCLCPP_INFO(node_->get_logger(),, "packet prease ok");
                Json::Reader reader;
                Json::Value value;
                std::string payload((const char*)packet.payload.data(), packet.size);
                // RCLCPP_INFO(node_->get_logger(),, "%s", payload.c_str());
                uint32_t encoder_value[2];

                if (reader.parse(payload, value)) {
                    // todo: parse json data
                    encoder_[0].value = value["encoder"]["1"].asUInt();
                    encoder_[1].value = value["encoder"]["2"].asUInt();
                    imu_.acc[0] = value["imu"][0]["ax"].asInt64() / 1000000.0;
                    imu_.acc[1] = value["imu"][0]["ay"].asInt64() / 1000000.0;
                    imu_.acc[2] = value["imu"][0]["az"].asInt64() / 1000000.0;

                    imu_.gyro[0] = value["imu"][1]["gx"].asInt64() / 1000000.0;
                    imu_.gyro[1] = value["imu"][1]["gy"].asInt64() / 1000000.0;
                    imu_.gyro[2] = value["imu"][1]["gz"].asInt64() / 1000000.0;

                    check_zero_crossing();
                    // RCLCPP_INFO(node_->get_logger(), "%ld, %ld", encoder_[0].value, encoder_[1].value);
                }
            }
        }

        p += size;
        // Remove the parsed packet from the buffer
        buffer_.erase(it, p);
    }
}

void TcpServerComm::handler_read(std::shared_ptr<boost::asio::ip::tcp::socket> socket)
{
    boost::system::error_code ec;
    int period = 1000 / loopHz_;
    stopped_ = false;
    uint8_t tmpbuf[BUFFER_MAX_LEN / 2];
    while(!stopped_) {
        size_t bytes_read = socket->read_some(boost::asio::buffer(tmpbuf, BUFFER_MAX_LEN / 2), ec);
        
        if(ec) {
            RCLCPP_WARN_STREAM(node_->get_logger(), "read: " << ec); 
            stop();
            return;
        } 
        
        if (bytes_read > 0)
        {
            std::copy(tmpbuf, tmpbuf + bytes_read, std::back_inserter(buffer_));
            // RCLCPP_INFO(node_->get_logger(),, "buffer size:%ld", buffer_.size());
            // check package length
            if (buffer_.size() > PACKHEAD_LENGTH) {
                last_data_time = std::time(0);
                ParsePacket();
            }
        }
        boost::this_thread::sleep(boost::posix_time::milliseconds(period));
        memset(tmpbuf, 0, sizeof(tmpbuf));
    }
    stopped_ = true;
}

void TcpServerComm::handler_write(std::shared_ptr<std::string> pstr,
    boost::system::error_code ec, size_t bytes_transferred) {
    if(ec)
        RCLCPP_WARN_STREAM(node_->get_logger(), "Send error:" << ec);
    else
        RCLCPP_INFO_STREAM(node_->get_logger(), "Send: " << *pstr);
    }

void TcpServerComm::update() 
{
   
}

void TcpServerComm::stop()
{
    stopped_ = true;
    if(status())
        sockPtr_->close();

    // if (service_run_)
    //     service_run_->join();


    if (parse_) {
        try {
            parse_->join();
        }
        catch(const std::exception& e) {
          RCLCPP_ERROR_STREAM(node_->get_logger(),"close parse thread catch err: "<< e.what());
        }
    }

    RCLCPP_INFO_STREAM(node_->get_logger(),"Tcp server close...");
}

void TcpServerComm::write(const std::string msg)
{
    // to do 
}

uint64_t TcpServerComm::read(const uint8_t index)
{
    assert(index < 2);
    return encoder_[index].value;
}

// TCP commuicate function implementation 
CanComm::CanComm(std::shared_ptr<rclcpp::Node> node,
     const std::string can_device, 
     const int nodeId, 
     const int protocol):
     Communication(node),
     can_device_(can_device), 
     nodeId_(nodeId), 
     protocol_(protocol)
{

}
        
CanComm::~CanComm()
{

}

int CanComm::init()
{
    driver_ = std::make_shared<can::ThreadedSocketCANInterface>();
    if (!driver_->init(can_device_, 0, can::NoSettings::create()))
    {
        RCLCPP_INFO_STREAM(node_->get_logger(),"Failed to initialize can_device " << can_device_.c_str());
        return -1;
    }
    else
    {
        RCLCPP_INFO_STREAM(node_->get_logger(),"Successfully connected to " << can_device_.c_str());
    }
    frame_listener_ = driver_->createMsgListenerM(this, &CanComm::msg_handle);

    sdoSend(nodeId_, 0x01, 2, 0x1800, 0x05, 0x14); // Transmission cycle is 20ms
    sdoSend(nodeId_, 0x01, 2, 0x1017, 0x00, 0x64); // Set heartbeat report period 100ms
    nmtSend(nodeId_, 0x01);
    return 0;
}

void CanComm::write(const std::string msg)
{
    /* set cable encoder position to default value */
    // uint32_t default_value=0;
    // sdoSend(nodeId_, 0x01, 4, 0x6003, 0x00, default_value); // preset value
    // sdoSend(nodeId_, 0x01, 4, 0x1010, 0x01, 0x65766173);    // save setting
}
            
uint64_t CanComm::read(const uint8_t index)
{
    return encoder_;
}

void CanComm::update()
{

}

void CanComm::nmtSend(uint8_t nodeId, uint8_t cmd)
{
    can::Frame can_frame;
    can_frame.id = 0x00;
    can_frame.is_extended = false;
    can_frame.is_rtr = false;
    can_frame.is_error = false;
    can_frame.dlc = 2;
    can_frame.data[0] = cmd;
    can_frame.data[1] = nodeId;
    driver_->send(can_frame);
}

void CanComm::sdoSend(uint8_t node, uint8_t rwflag, uint8_t len, uint16_t index, uint8_t subIndex, uint32_t data)
{
    can::Frame can_frame;
    can_frame.id = (0x600 + node);
    can_frame.is_extended = false;
    can_frame.is_rtr = false;
    can_frame.is_error = false;
    can_frame.dlc = 8;
    for(int i=0; i<can_frame.dlc; i++)
    {
        can_frame.data[i] = 0;
    }
    if (rwflag)
    {
        switch(len)
        {
            case 1:
                can_frame.data[0] = 0x2f;
                break;
            case 2:
                can_frame.data[0] = 0x2b;
                break;
            case 3:
                can_frame.data[0] = 0x27;
                break;
            case 4:
                can_frame.data[0] = 0x23;
                break;
            default:break;
        }
    }
    else
    {
        can_frame.data[0] = 0x40;
    }
    can_frame.data[1] = index;
    can_frame.data[2] = index >> 8;
    can_frame.data[3] = subIndex;
    for(int i=0; i<len; i++)
    {
        can_frame.data[4+i] = (data >> (8 * i)) & 0xff;
    }
    driver_->send(can_frame); 
}

void CanComm::msg_handle(const can::Frame &msg)
{
    if (!msg.isValid())
    {
        RCLCPP_ERROR(node_->get_logger(), 
            "Invalid frame from SocketCAN: id: %#04x, length: %d, is_error: %d", msg.id, msg.dlc, msg.is_error);
        return;
    }
    // switch (protocol_)
    if ((msg.id - 0x180) == nodeId_)
    {
        encoder_ = msg.data[0] | msg.data[1] << 8 ; //uint=0.1mm
    }
}

}// end namespace
